//----------------------------------------------------------------------
/*!\file
 *
 * \author  
 * \date   
 *
 */
//----------------------------------------------------------------------
#include <hardware_interface.h>

// bitset mask is applied to robot safety status bits in order to determine 'in_error' state

HardwareInterface::HardwareInterface()
{
  position_controller_running_ = false;
  controller_reset_necessary_  = true;
  
  cmd.resize(5);
  pos.resize(6);
  vel.resize(6);
  eff.resize(6);
  joint_names_.resize(6);

  cmd[0]=-999;  //设置不可达的关节角作为JointGroupPosition控制器启动未完成的标志位

}
HardwareInterface::~HardwareInterface()
{
  usleep(1);
}

bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
{
  double Max_A,Max_V;
  robot_hw_nh.getParam("joints", joint_names_);
  robot_hw_nh.getParam("MaxA", Max_A);
  robot_hw_nh.getParam("MaxV", Max_V);
  robot_hw_nh.getParam("useRealRobot", useRealRobot);

/***************************
*/
if(useRealRobot)
{
  ROS_INFO_STREAM("Initializing real robot");
}
/*
************************/
 
  // Create ros_control interfaces
  for (std::size_t i = 0; i < 6; ++i)
  {
    ROS_DEBUG_STREAM("Registering handles for joint " << joint_names_[i]);
    // Create joint state interface for all joints
    //更新关节状态时，对读取的值依次赋给每个关节，并发布到/joint_status,但发布时会按首字母顺序进行排序

    js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &pos[i],
                                                                      &vel[i], &eff[i]));
  }

  for (std::size_t i = 0; i < 5; ++i)
  {
/*                                                                 
    Create joint position control interface
      * controler初始化时，将对yaml文件中的关节名与注册时的关节名进行检查（不论顺序，都有就行）
      * controler初始化成功以后，将自动执行cmd赋值，顺序为注册时名字的顺序，对应值付给cmd，而不是字母排序后的顺序
      * 接收指令时从topic接收，也会进行关节名检查，按注册时的关节名与yaml中的关节名进行对应赋值，顺序为注册时的名字顺序
      
      * 例：注册时 {"shoulder_joint", "upperArm_joint","foreArm_joint" ,"wrist1_joint", "wrist2_joint", "wrist3_joint"}
        依次为关节1到6，且实际关节角 [1,2,3,4,5,6]
      * 发布时 {"foreArm_joint", "shoulder_joint","upperArm_joint" ,"wrist1_joint", "wrist2_joint", "wrist3_joint"}
              [3,1,2,4,5,6]
      * 第一次赋值cmd [1,2,3,4,5,6]
      * yaml文件中的关节名 {"foreArm_joint", "shoulder_joint","upperArm_joint" ,"wrist1_joint", "wrist2_joint", "wrist3_joint"}
      * 接收数组    [10,20,30,40,50,60]
      * 实际赋值cmd [20,30,10,40,50,60]
*/
    pj_interface_.registerHandle(
        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &cmd[i]));
  }
  // robot_status_interface_.registerHandle(industrial_robot_status_interface::IndustrialRobotStatusHandle(
  //     "industrial_robot_status_handle", robot_status_resource_));

  // Register interfaces
  registerInterface(&js_interface_);
  registerInterface(&pj_interface_);
  return true;
}

bool HardwareInterface::checkControllerClaims(const std::set<std::string>& claimed_resources)
{
  for (const std::string& it : joint_names_)
  {
    for (const std::string& jt : claimed_resources)
    {
      if (it == jt)
      {
        return true;
      }
    }
  }
  return false;
}

void HardwareInterface::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                                 const std::list<hardware_interface::ControllerInfo>& stop_list)
{
  std::cout<<"doSwitch"<<std::endl;
  //todo: stop list
    for (auto& controller_it : stop_list)
  {
    for (auto& resource_it : controller_it.claimed_resources)
    {
      if (checkControllerClaims(resource_it.resources))
      {
        if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
        {
          position_controller_running_ = false;
          // cout<<"position_controller_running_ is false"<<endl;
        }
      }
    }
  }
  for (auto& controller_it : start_list)
  {
    for (auto& resource_it : controller_it.claimed_resources)
    {
      if (checkControllerClaims(resource_it.resources))
      {
        if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
        {
          position_controller_running_ = true;
          // cout<<"position_controller_running_ is true"<<endl;
        }
      }
    }
  }

}

bool HardwareInterface::shouldResetControllers()
{
  if (controller_reset_necessary_)
  {
    controller_reset_necessary_ = false;
    return true;
  }
  else
  {
    return false;
  }
}

void HardwareInterface::getJointStatus(vector<double> &JP)
{
  usleep(1);
}

void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
{
  if (position_controller_running_ && cmd[0]!=-999)
  {
    for (int8_t i = 0; i < 5; ++i)
    {
      pos[i] = cmd[i];
    }
  }
}

void HardwareInterface::write(const ros::Time& time, const ros::Duration& period)
{
  if (position_controller_running_ && cmd[0]!=-999)
  {
    if(useRealRobot)
    {
      usleep(1);
    }
  }
}